/***************************************************************************//**
 * @file camera_array_calibration.cpp
 * @brief Determines the camera matrix and focal length for each camera
 *
 * @page "Camera Calibration" Camera Calibration
 * 
 * @title
 *
 * @authors Brian Fehrman
 * 
 * @date April 17 2013
 * 
 * @section file_section File Information
 *
 * @details Implementation file for the camera_calibration functions
 *       which are used to determine the camera matrix and the focal
 *       length for up to NxM cameras simultaneously. The information for
 *       each camera is then saved to an XML file, with one XML file being
 *       generated for each camera. Uses the chessboard calibration routine
 *       to find the needed data. This expects that the images captured for
 *       each camera will be stored in the format:
 * 
@verbatim
base_dir/folder_names[ i ]/jimg_suf
@endverbatim
 * 
 *       Where i is the name of the folder for the current camera image set
 *       and j is some integer number which should be 0 for the first image
 *       in the set and increment by 1 for each additional image in the set.
 *       This is the format that the camera capture routine uses to save the
 *       images that is captures. For instance:
@verbatim
base_dir/folder_names[ i ]/0img_suf
base_dir/folder_names[ i ]/1img_suf
...
base_dir/folder_names[ i ]/nimg_suf
@endverbatim
 * 
 * @section settings_section Required Settings
 * 
@verbatim
[ "board_width" ] - Number of inner corners along the width of the board (int)
[ "board_height" ] - Number of inner corners along the height of the board (int)
[ "base_dir" ] - name of the directory which contains the images to be used for calibration
[ "img_suf" ] - the suffix that should be common to all images used for calibration;
[ "square_size" ] - size of the chessboard squares in mm;
[ "folder_names" ] - names of the folders inside of base_dir that contain images to use for calibration;
[ "calib_dir" ] - directory where calibration information should be saved
@endverbatim
 * 
 * @section todo_bugs_modification_section Todo, Bugs, and Modifications
 *
 * @todo
 ******************************************************************************/

////////// Includes //////////////
#include "camera_array_calibration.h"

/***************************************************************************//**
 * @author Brian Fehrman
 *
 * @par Description:
 * Calibrate cameras based on a given set of images. Can
 * calibrate any number of image sets. Stores the results
 * in the calib_dir folder specified by the user in the settings file.
 *
 * @param[in] fs - FileNode pointing to the settings that are needed
 *
 * @returns 0 - everything went fine
 ******************************************************************************/
int calibrate( FileNode fs )
{
   Size board_size;
   string base_dir;
   string img_suf;
   vector< string > folder_names;
   int square_size;
   string calib_dir;
   
   fs[ "board_width" ] >> board_size.width;
   fs[ "board_height" ] >> board_size.height;
   fs[ "base_dir" ] >> base_dir;
   fs[ "img_suf" ] >> img_suf;
   fs[ "square_size" ] >> square_size;
   fs[ "folder_names" ] >> folder_names;
   fs[ "calib_dir" ] >> calib_dir;

   vector< vector<Point3f> > obj_points(1);
   
   base_dir += "/";

   mkdir( calib_dir.c_str(), S_IRWXU|S_IRGRP|S_IXGRP );
   
   calib_dir += "/";
   
   //Approximates position of board corners, needed for calibration
   for( int i = 0; i < board_size.height; ++i )
      for( int j = 0; j < board_size.width; ++j )
         obj_points[ 0 ].push_back(Point3f(float( j*square_size ), float( i*square_size ), 0));

   //Calibrate each image set and save the results
   #pragma omp parallel for
   for( unsigned int img_set = 0; img_set < folder_names.size(); img_set++ )
   {
      vector< vector< Point2f > > all_corners;
      Mat camera_mat = Mat::eye(3, 3, CV_64F );
      Mat dist_coeffs = Mat::zeros( 8, 1, CV_64F );
      vector<Mat> r_vecs, t_vecs;
      vector<Mat> image_set;
      double rms = 0;
      string img_base = base_dir + folder_names[ img_set ] + "/";
      FileStorage calib_save( calib_dir + folder_names[ img_set ] + ".xml", FileStorage::WRITE );
      
      cout << "Calibrating set: " << img_base << endl;
     
      cout << "Finding corners...." << endl;
      
      find_corners( img_base, img_suf, board_size, all_corners, image_set );
      
      if( all_corners.size() > 0 )
      {
         cout << "Succesfully found corners" << endl;
      }
      else
      {
         cout << "Error finding corners" << endl;
         continue;
      }
      
      obj_points.resize( all_corners.size(), obj_points[ 0 ] );
      
      rms = calibrateCamera( obj_points, all_corners, image_set[ 0 ].size(),  camera_mat,
                              dist_coeffs, r_vecs, t_vecs );
                              
      cout << "RMS on Calibration: " << rms << endl;
      
      calib_save << "camera_mat" << camera_mat;
      calib_save << "dist_coeffs" << dist_coeffs;
      calib_save << "r_vecs" << r_vecs;
      calib_save << "t_vecs" << t_vecs;
      calib_save << "rms" << rms;
      
      cout << endl << endl;
      
      calib_save.release();
   }
   
   return 0;
}

/***************************************************************************//**
 * @author Brian Fehrman
 * 
 * @par Reference:  http://docs.opencv.org/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
 *
 * @par Description:
 * Find the inner corners in one or more images containg a chessboard
 * and return those corners. It expects the pictures to be named 
 * img_base/#img_suf where # is the number of the picture in the set.
 * The number should start at 0 and increment by 1 for each image. For instance:
@verbatim
img_base/0img_suf
img_base/1img_suf
img_base/2img_suf
...
img_base/nimg_suf
@endverbatim
 * This will find the corners in each image in the set and return the points
 * so that tey can be used for calibration. It continues until a file read
 * error is encountered at which case it assumes that there are no more images
 * in the directory and this is why it is important that the images are named
 * using the convention outlined above.
 *
 * @param[in] img_base - Name of the base folder
 * @param[in] img_suf - Common suffix of all images
 * @param[in] board_size - Number of inner corners in (col, row) format
 * @param[out] all_corners - Stores the corners found for each image in the set
 * @param[out] image_set - Stores each of the images read in to be used in calibration
 *
 * @returns 0 - everything went fine
 ******************************************************************************/
int find_corners( string img_base, string img_suf, Size board_size,
                     vector< vector< Point2f > > &all_corners, vector<Mat> &image_set )
{   
   int img = 0;
   bool image_found = true;
   string last_image_read;
   
   //For each image in the set, find the corners
   //Continues until an image read is unsuccesful
   do
   {
      Mat image;
      stringstream ss;
      string image_name;
      vector<Point2f> image_corners;
      
      ss << img;
      
      image_name = img_base + ss.str() + img_suf;
      image = imread(  image_name );
      
      if( !image.data )
      {
         if( img > 0 )
         {
            cout << "Final image in set was: " << last_image_read << endl;
         }
         else
         {
            cout << "No images found in set" << endl;
         }
         image_found = false;
      }
      
      else
      {
         bool found = findChessboardCorners( image,
                              board_size, image_corners); //, CV_CALIB_CB_NORMALIZE_IMAGE );
         
         if( !found )
            cout << "Problem finding corners for img: " << img << endl;
         else
         {
            //The cornerSubPix functions improves pixel accuracy
            Mat gray_conv;
            cvtColor( image, gray_conv, CV_BGR2GRAY);
            cornerSubPix( gray_conv, image_corners, Size(11,11),
                        Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
                        
            all_corners.push_back( image_corners );
            image_set.push_back( image );
         }
      }
      
      img++;
      last_image_read = image_name;
      
   }while( image_found );
   
   return 0;
}
